#include "count.h"

#define BAS2	32260
#define	BASE	BAS2*2
#define	DSLEEP	10000
#define	RING1	4
#define	RING2	8

static int	smax=0x80000000,smin=0x7fffffff;
static byte	time=100;

static void resetMax(){
	smax=0x80000000;
	smin=0x7fffffff;
	time=100;
}
static ubyte window(void){
	static byte	wnd,wod;
	if((smax-smin)>DSLEEP){
		wod=wnd;
		wnd=100-time;
		resetMax();
		if(wnd>10){
			if(!wod)
				return 1;
			byte d=wod-wnd;
			if(d<0)
				d=-d;
			if(d<wnd/4)
				return 1;
		}
	}
	wnd=0;
	return 0;
}

//------------------------------------------------------------------------------

ubyte count(int x,int y,int z,ubyte stt){
	static byte	cnt=0;
	static int	cut=BAS2*4;
	static int	ring1[RING1]={BASE,BASE,BASE,BASE,};
	static int	sum1=BAS2*RING1*2;
	static int	ths=BAS2*RING1*13/6;
	static int	*shead=ring1+RING1-1,*stail=ring1;
	static byte	told,sum2;
	static byte	ring2[RING2];
	static byte	*thead=ring2+RING2-1,*ttail=ring2;
	x>>=5;
	y>>=5;
	z>>=5;
	int w=x*x+y*y+z*z;
	if(w>cut)
		w=cut;
	if(smax<w)
		smax=w;
	if(smin>w)
		smin=w;
	sum1+=w-*stail;
	*shead=w;
	w=sum1<ths?0:1;
	sum2+=w-*ttail;
	*thead=w;
	if(sum2<RING2/2)
		told=0;
	else{
		if(!told){
			if(window()){
				if(stt==COUNT_STEPING)
					curSteps++;
				else if(cnt>2){
					stt=COUNT_STEPING;
					curSteps+=cnt+2;
					cnt=0;
				}else cnt++;
			}else{
				stt=COUNT_SEARCHING;
				cnt=0;
			}
		}
		told=1;
	}
	if(!(--time)){
		resetMax();
		stt=(stt==COUNT_SEARCHING||stt==COUNT_STANDBY)?COUNT_STANDBY:
			COUNT_SEARCHING;
		cnt=0;
	}
	if(++shead>=(ring1+RING1))
		shead=ring1;
	if(++stail>=(ring1+RING1))
		stail=ring1;
	if(++thead>=(ring2+RING2))
		thead=ring2;
	if(++ttail>=(ring2+RING2))
		ttail=ring2;
	return stt;
}